#Region ;**** Directives created by AutoIt3Wrapper_GUI ****
#AutoIt3Wrapper_Res_requestedExecutionLevel=asInvoker
#EndRegion ;**** Directives created by AutoIt3Wrapper_GUI ****
#include <GUIConstantsEx.au3>
#include "CommMG.au3"


;==============================================
;==============================================
;SERVER!! Start Me First !!!!!!!!!!!!!!!
;==============================================
;==============================================
Global Const $M_PI =  3.14159265358979323846
Global Const $M_PI_2 =  1.57079632679489661923
Global Const $M_PI_4 =  0.785398163397448309616

Example()


Func Example()
    ; Set Some reusable info
    ; Set your Public IP address (@IPAddress1) here.
;   Local $szServerPC = @ComputerName
;   Local $szIPADDRESS = TCPNameToIP($szServerPC)
    Local $szIPADDRESS = @IPAddress1
    Local $nPORT = 33891
    Local $MainSocket, $GOOEY, $edit, $ConnectedSocket, $szIP_Accepted
    Local $msg, $renpcvnp
	Local $recievedString
	Local $scalingFactor, $joyAngle, $joyPosition
	$error = ""
	;$ports = _ComGetPortNames()
	;MsgBox(0, "", $ports[0][0])
	;MsgBox(0, "", "Sent message")
	_CommSetPort("7", $error, 9600)
    ; Start The TCP Services
    ;==============================================
    TCPStartup()

    ; Create a Listening "SOCKET".
    ;   Using your IP Address and Port 33891.
    ;==============================================
    $MainSocket = TCPListen($szIPADDRESS, $nPORT)

    ; If the Socket creation fails, exit.
    ;If $MainSocket = -1 Then Exit


    ; Create a GUI for messages
    ;==============================================
    $GOOEY = GUICreate("My Server (IP: " & $szIPADDRESS & ")", 300, 250)
    $edit = GUICtrlCreateEdit("", 10, 10, 280, 180)
	$IPLabel = GUICtrlCreateLabel(@IPAddress1, 10, 200, 280, 60, 0x01)
		GUICtrlSetFont($IPLabel, 28, 1600)
    GUISetState()


    ; Initialize a variable to represent a connection
    ;==============================================
    $ConnectedSocket = -1


    ;Wait for and Accept a connection
    ;==============================================
    Do
        $ConnectedSocket = TCPAccept($MainSocket)
    Until $ConnectedSocket <> -1

	TCPSend($ConnectedSocket, "Connected")
    ; Get IP of client connecting
    $szIP_Accepted = SocketToIP($ConnectedSocket)

    ; GUI Message Loop
    ;==============================================
    While 1
        $msg = GUIGetMsg()

        ; GUI Closed
        ;--------------------
        If $msg = $GUI_EVENT_CLOSE Then ExitLoop

        ; Try to receive (up to) 2048 bytes
        ;----------------------------------------------------------------
        $recv = TCPRecv($ConnectedSocket, 2048)

        ; If the receive failed with @error then the socket has disconnected
        ;----------------------------------------------------------------
        If @error Then
			$ConnectedSocket = -1
			Do
				$ConnectedSocket = TCPAccept($MainSocket)
			Until $ConnectedSocket <> -1
		EndIf

        ; Update the edit control with what we have received
        ;----------------------------------------------------------------
	  If $recv <> "" Then
		  ;Added
		  $recievedString= StringSplit($recv, "|")
		  If($recievedString[1] == "E")Then
			_CommSendString("E")
		  ElseIf($recievedString[1] == "M")Then
			$scalingFactor = $recievedString[2]
			$joyAngle= $recievedString[3]
			$joyPosition = $recievedString[4]
			TankTurning($edit, $szIP_Accepted, $scalingFactor, $joyAngle, $joyPosition )
			EndIf
	  EndIf
	$serial = ""
	Sleep(150)
	$serial = _CommGetString()
	If $serial <> "" Then
		GUICtrlSetData($edit,  "Serial > " & $serial & @CRLF & GUICtrlRead($edit))
	EndIf
    WEnd

    If $ConnectedSocket <> -1 Then TCPCloseSocket($ConnectedSocket)

    TCPShutdown()
EndFunc   ;==>Example

; Function to return IP Address from a connected socket.
;----------------------------------------------------------------------
Func SocketToIP($SHOCKET)
    Local $sockaddr, $aRet

    $sockaddr = DllStructCreate("short;ushort;uint;char[8]")

    $aRet = DllCall("Ws2_32.dll", "int", "getpeername", "int", $SHOCKET, _
            "ptr", DllStructGetPtr($sockaddr), "int*", DllStructGetSize($sockaddr))
    If Not @error And $aRet[0] = 0 Then
        $aRet = DllCall("Ws2_32.dll", "str", "inet_ntoa", "int", DllStructGetData($sockaddr, 3))
        If Not @error Then $aRet = $aRet[0]
    Else
        $aRet = 0
    EndIf

    $sockaddr = 0

    Return $aRet
EndFunc   ;==>SocketToIP

;ADDED THIS MARCH 2, 2012 KEEP THIS
Func TankTurning($edit, $szIP_Accepted, $scalingFactor, $joystickAngle, $position)
	Local $rightSpeedCommand
	Local $leftSpeedCommand
	Local $degreeFreedom  =(2/180)*$M_PI
	Local $degreeFreedomPI_4 = (1/180)*$M_PI
	;Checks if parameters are appropriate, if not kills motors
	if(($scalingFactor <= 0 or $scalingFactor > 32) or($joystickAngle < 0 or $joystickAngle > (2*$M_PI)) or ($position <= 10 or $position > 100)) Then
		_CommSendString("ML+0000R+0000")
	Else

		;Checks if the joystick is at 45, -45, 135 or -135
		If($joystickAngle >= ($M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=($M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		ElseIf($joystickAngle >= (7*$M_PI_4-$degreeFreedomPI_4) and $joystickAngle <=(7*$M_PI_4+$degreeFreedomPI_4))Then
			_CommSendString("ML+0000R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 3*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 3*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML+0000R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle >= 5*$M_PI_4-$degreeFreedomPI_4 and $joystickAngle <= 5*$M_PI_4+$degreeFreedomPI_4)Then
			_CommSendString("ML-" & String(StringFormat("%04s",$position*$scalingFactor)) & "R" & "+0000")

		;Checks if the joystick is at 0 or 180 degrees
		ElseIf($joystickAngle > -$degreeFreedom and $joystickAngle < $degreeFreedom) Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < $M_PI+$degreeFreedom and $joystickAngle > $M_PI-$degreeFreedom) Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		;Checks if the joystick is at 90 or -90 degrees
		ElseIf($joystickAngle < $M_PI_2+$degreeFreedom and $joystickAngle > $M_PI_2-$degreeFreedom)Then
			_CommSendString("ML+" & String(StringFormat("%04s", $position*$scalingFactor)) & "R+" & String(StringFormat("%04s",$position*$scalingFactor)))

		ElseIf($joystickAngle < 3*$M_PI_2+$degreeFreedom and $joystickAngle > 3*$M_PI_2-$degreeFreedom)Then
			_CommSendString("ML-" & String(StringFormat("%04s", $position*$scalingFactor)) & "R-" & String(StringFormat("%04s",$position*$scalingFactor)))

		Else

			$joystickAngle=($joystickAngle-$M_PI_4)
			;If joystick is between Pi/4 and Pi/2
			If($joystickAngle > $degreeFreedomPI_4 and $joystickAngle < $M_PI_4-$degreeFreedom) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi/2 and 3Pi/4
			ElseIf($joystickAngle > $M_PI_4+$degreeFreedom and $joystickAngle < $M_PI_2-$degreeFreedomPI_4) Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/4 and Pi
			ElseIf($joystickAngle > $M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 3*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-"& String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between Pi and 5Pi/4
			ElseIf($joystickAngle > 3*$M_PI_4+$degreeFreedom and $joystickAngle < $M_PI-$degreeFreedomPI_4)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R+" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 5Pi/4 and 3Pi/2
			ElseIf($joystickAngle > $M_PI+$degreeFreedomPI_4 and $joystickAngle < 5*$M_PI_4-$degreeFreedom)Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 3Pi/2 and 7Pi/4
			ElseIf($joystickAngle > 5*$M_PI_4+$degreeFreedom and $joystickAngle < 3*$M_PI_2-$degreeFreedomPI_4)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML-" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 7Pi/4 and 2Pi
			ElseIf($joystickAngle > 3*$M_PI_2+$degreeFreedomPI_4 and $joystickAngle < 7*$M_PI_4-$degreeFreedom)Then
				$rightSpeedCommand= $position*$scalingFactor
				$leftSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))
			;If joystick is between 0 and Pi/4
			ElseIf($joystickAngle > -$M_PI_4+$degreeFreedom and $joystickAngle < 0-$degreeFreedomPI_4) Then
				$leftSpeedCommand= $position*$scalingFactor
				$rightSpeedCommand= -1*Floor($position*$scalingFactor*Sin(2*$joystickAngle))
				_CommSendString("ML+" & String(StringFormat("%04s", $leftSpeedCommand)) & "R-" & String(StringFormat("%04s", $rightSpeedCommand)))

			EndIf
		EndIf
	EndIf
EndFunc

;Used For Testing
Func decimalToBinary($decimalVal)
	Local $array[11]
	Local $count
		For $count=0 to 10
			If(Mod($decimalVal, 2) == 1)Then
				$array[$count] = 1
			Else
				$array[$count] = 0
			EndIf
			$decimalVal = Floor($decimalVal/2)
		Next
	Return $array
EndFunc